/*
 * Copyright (c) 2010 Network Security Lab (NSL), Department of Electrical
 *                    Engineering, University of Washington
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation;
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * Author: He Wu mdzz@uw.edu
 */

/**
 * @file router.c
 */

#include <string.h>
#include <unistd.h>
#include <stdio.h>
#include <pthread.h>
#include <assert.h>
#include "router.h"
#include "socket.h"

// debug switch
#define routing_debug 0

// global
static int numHops = 255;       //!< number of hops away from base
static toBaseRoutingEntry toBaseTable[ROUTING_TABLE_SIZE];  //!< node-to-base routing table
static pthread_mutex_t toBaseRtTableLock = PTHREAD_MUTEX_INITIALIZER;

static toNodeRtTable to_node_table;

//
extern int sendCommand; //!< send command flag

/******************************************************************************/

/**
 *
 */
static void append_toNodeRtEntry(toNodeRtTable *table, __u8 length, __u16 destAddr, __u16 *addrList);

/**
 * @brief Checks to node routing table and remove old entries.
 * @param table Pointer to base-to-node routing table.
 *
 * Not thread safe, to be called within @see append_toNodeRtEntry only.
 */
static void check_toNodeRtTable(toNodeRtTable *table);

/**
 * @brief Finds routing table entry to the specified address.
 * @param h Pointer to the head of routing table (linked list).
 * @param addr Address.
 * @return Pointer to the routing table entry (linked list node).
 *
 * Returned pointer is read only, to protect the content of the routing table.
 */
static const toNodeRtEntry *findRtToNode(toNodeRtTable *table, __u16 addr);

/**
 * @brief Report base-to-node routing table to GUI. Thread safe.
 */
static void reportRoutintTable(toNodeRtTable *table);

/**
 * @brief Updates node-to-base routing table.
 * @param src_node Information about sender of routing packet.
 * @param canHear List of nodes the sender of routings can hear.
 *
 * The entries of node-to-base routing table are replaced using the following
 * rules:
 *  1. If the sender can hear the current node, replace the first entry that is
 *  further away from base node then the sender.
 *  2. If the sender cannot hear the current node, replace an empty entry with
 *  the sender.
 *
 * The node-to-base routing table is always sorted. The first entry is closest
 * to the base node.
 */
static void updateToBaseTable(nodeProperty src_node, __u16 *canHear);

/**
 * @brief Process routing packet at the base node.
 * @param in_pkt Pointer to incoming packet.
 */
static void base_recv_rtPkt(packet *in_pkt);

/**
 * @brief Sends node-to-base routing packet.
 * @return 0 if all is good.
 */
static int send_to_base_rtPkt();

/**
 * @brief Sends base-to-node routing packet back to base node.
 * @return 0 if all is good.
 */
static int send_to_node_rtPkt();

/**
 * @brief Finds the shortest route to base node.
 * @return Address of the shortest route to base. If not found, 0x0 is returned.
 */
static __u16 findRtToBase();

/******************************************************************************/

void initTable()
{
  int i;

  // initialize table to "0000 255 0"
  for (i = 0; i < ROUTING_TABLE_SIZE; i++) {
    toBaseTable[i].nextHop = 0x0;
    toBaseTable[i].numHops = 255;
    toBaseTable[i].lastHeard = 0;
  }

  if (check_base()) {
    numHops = 0;
  }
  else
    numHops = 255;

  // initialize base to node routing table (linked list)
  to_node_table.head = NULL;
  to_node_table.size = 0;
  pthread_mutex_init(&to_node_table.lock, NULL);
}


void routingBeacon()
{
  int i;
  __u32 interval; // actual interval for beacon

  while (1) {
    // stop beaconing routing info when sending commands
    if (!sendCommand) {
      // send route to base packet
      for (i = 0; i < TO_BASE_ROUTING_RESEND; i++) {
        if (send_to_base_rtPkt() < 0)
          printf("Error sending routing packets.\n");
        // wait before next send
        if (i >= 0)
          usleep(WAIT_TIME);
      }

      // calculate skewed interval
      interval = skew(ROUTING_BEACON_INTERVAL);

      if (routing_debug)
        printf("Routing beacon interval = %d\n", interval);

      // check and remove old base-to-node routing entries
      check_toNodeRtTable(&to_node_table);

      usleep(interval);

      // send route to node packet
      if (!check_base()) {
        for (i = 0; i < TO_NODE_ROUTING_RESEND; i++) {
          if (send_to_node_rtPkt() < 0)
            printf("Error sending routing packets.\n");
          // wait before next send
          if (i >= 0)
            usleep(WAIT_TIME);
        }
      }

      // calculate skewed interval
      interval = skew(ROUTING_BEACON_INTERVAL);
      if (routing_debug)
        printf("Routing beacon interval = %d\n", interval);

      // check and remove old base-to-node routing entries
      check_toNodeRtTable(&to_node_table);

      usleep(interval);
    }
  }
}


int sendtoBase(packet *out_pkt, int ACK_en)
{
  __u16 destNode = findRtToBase();

  // check if path exists
  if (destNode == 0x0) {
    printf("No path to base.\n\n");
    return -1;
  }

  // send pkt
  printf("Sending packet back to base through: %x\n", destNode);
  if (h_send(destNode, out_pkt, ACK_en) < 0)
    return -1;

  return 0;
}


int sendtoNode(__u16 destAddr, packet *out_pkt, int ACK_en)
{
  int i;
  packet tmp_pkt;
  __u16 nextHop;
  const toNodeRtEntry *target = findRtToNode(&to_node_table, destAddr);

  // initialize packet
  initPkt(&tmp_pkt, CMD_PKT_TYPE);

  // move cmd data to the last 12 bytes
  memcpy(tmp_pkt.data + 14, out_pkt->data, 12);

  // fill in routing section
  if (target == NULL) {
    printf("No path to node %x.\n\n", destAddr);
    return -1;
  }
  else {
    printf("Found path to node %x.\n", target->destAddr);
    printf("%d hops needed.\n", target->length - 1);
  }
  tmp_pkt.data[0] = target->length;
  tmp_pkt.data[1] = (__u8)(target->destAddr >> 8);  // fill destination address
  tmp_pkt.data[2] = (__u8)target->destAddr;

  for (i = 0; i <= target->length - 2; i++) { // fill the routing list
    tmp_pkt.data[3 + 2 * i] = (__u8)(target->addrList[i] >> 8);
    tmp_pkt.data[4 + 2 * i] = (__u8)target->addrList[i];
  }
  if ((target->length - 1) == 0) // 1 hop away
    nextHop = target->destAddr;
  else
    nextHop = target->addrList[target->length - 2];

  // send packet
  printf("Sending packet to %x through: %x\n", destAddr, nextHop);
  if (h_send(nextHop, &tmp_pkt, ACK_en) < 0)
    return -1;

  // print out each byte of the command packet
  if (routing_debug) {
    printf("\n");
    for (i = 0; i < 26; i++) {
      printf("To node command packet content is: %x\n", tmp_pkt.data[i]);
    }
    printf("\n");
  }
  return 0;
}


void recv_rtPkt(packet *in_pkt)
{
  __u8 *tmp;
  __u16 canHear[ROUTING_TABLE_SIZE];  // table of reachable nodes
  nodeProperty src_node;
  int i;

  // get srcAddr, srcHop
  memcpy(&(src_node.addr), in_pkt->data, 2);
  src_node.numHops = in_pkt->data[2];
  if (routing_debug) {
    printf("\nRouting packets from: %x, %d hops away from base node.\n",
           src_node.addr, src_node.numHops);
  }

  // get all neighbor address
  tmp = in_pkt->data + 3;
  for (i = 0; i < ROUTING_TABLE_SIZE; i++) {
    memcpy(&(canHear[i]), tmp, 2);
    tmp += 2;
    if (routing_debug)
      printf("Neighbor address are: %x\n", canHear[i]);
  }

  // update routing table
  updateToBaseTable(src_node, canHear);
}


int forward_rt_toBase(packet *out_pkt)
{
  int i;
  __u16 localAddr;

  // get localAddr
  localAddr = get_addr();

  // process packet if currently at base node
  if (check_base()) {
    printf("Routing packets have reached base node!\n\n");
    base_recv_rtPkt(out_pkt);
  }
  // forward message to next hop
  else {
    out_pkt->senderAddr = localAddr;
    out_pkt->data[0]++; // update # of nodes in packet
    i = (out_pkt->data[0] - 1) * 2;
    // stop if maximum number exceeded
    if (i > (MAX_NUM_NODES_IN_RT * 2)) {
      printf("Maximum number of nodes reached!\n");
      return -1;
    }
    // NOTICE bitwise operation
    // attach current node address to the END of the list
    out_pkt->data[i + 1] = (__u8)(localAddr >> 8);
    out_pkt->data[i + 2] = (__u8)localAddr;
    if (sendtoBase(out_pkt, 0) < 0)
      return -1;
  }

  return 0;
}


void displayRoutingTable()
{
  int i;
  const toNodeRtEntry *p;

  while (1) {
    // show node-to-base routing table
    if (!check_base()) {
      printf("\nNode-to-base routing table is:\n");
      for (i = 0; i < ROUTING_TABLE_SIZE; i++)
        printf("%x\t%d\t%d\n", toBaseTable[i].nextHop, toBaseTable[i].numHops,
               toBaseTable[i].lastHeard);
      printf("\n");
    }
    else {
      // show to-node routing table
      printf("Base-to-nodes routing table is:\n");
      pthread_mutex_lock(&to_node_table.lock);
      for (p = to_node_table.head; p; p = p->next) {
        printf("Address: %x, %d hops away, last heard: %d. ", p->destAddr,
               p->length, p->age);
        for (i = 0; i < p->length - 1; i++)
          printf("%x ", p->addrList[i]);
        printf("\n");
      }
      pthread_mutex_unlock(&to_node_table.lock);
      printf("\n");
      // report routing table to GUI
      reportRoutintTable(&to_node_table);
    }
    sleep(TABLE_DISPLAY_INTERVAL);  // display interval
  }
}

/******************************************************************************/

static void reportRoutintTable(toNodeRtTable *table)
{
  assert(table);
  int j, size;
  __u16 routingTable[127]; // at most 127 entries
  const toNodeRtEntry *p;

  // build routing table to send to GUI
  pthread_mutex_lock(&table->lock);
  for (p = table->head, j = 0; p; p = p->next, j++) {
    routingTable[j] = p->destAddr;
    assert(j <= 127);
  }
  size = table->size;
  pthread_mutex_unlock(&table->lock);

  // send to node routing table back to GUI
  routingSocket(routingTable, size);
}


static int send_to_base_rtPkt()
{
  int i;
  packet out_pkt; // out going packet
  __u16 localAddr;

  // get localAddr
  localAddr = get_addr();

  // initialize packet
  initPkt(&out_pkt, ROUTING_PKT_TYPE);

  // build data payload
  memcpy(out_pkt.data, &localAddr, 2);
  if (!check_base())
    out_pkt.data[2] = numHops;
  else
    out_pkt.data[2] = 0;  // base node is 0 hops from base
  if (routing_debug) {
    printf("Current node address is: %x\n", out_pkt.data[0]);
    printf("Num of hops is: %d\n", out_pkt.data[2]);
  }

  /* entering critical section */
  pthread_mutex_lock(&toBaseRtTableLock);
  // read routing table into routing packet
  for (i = 0; i < ROUTING_TABLE_SIZE; i++) {
    memcpy(out_pkt.data + 2 * i + 3, &(toBaseTable[i].nextHop), 2);
    if (routing_debug)
      printf("Can hear: %x\n", out_pkt.data[2 * i + 3]);
  }
  pthread_mutex_unlock(&toBaseRtTableLock);
  /* leaving critical section */

  // send back-to-base routing packet
  if (h_send(BROADCAST_ADDR, &out_pkt, 0) < 0) {
    printf("Failed sending to-base routing packet!\n\n");
    return -1;
  }
  else
    printf("Back to base routing packets sent.\n\n");

  return 0;
}


static int send_to_node_rtPkt()
{
  int i;
  packet out_pkt;
  __u16 destNode, localAddr;

  // get localAddr
  localAddr = get_addr();

  // read local to base routing table
  for (i = 0; i < ROUTING_TABLE_SIZE; i++) {
    // first non-zero entry is always the shortest
    destNode = toBaseTable[i].nextHop;
    if (destNode != 0x0)
      break;
  }
  // check if path exists
  if (destNode == 0x0) {
    printf("No path to base.\n\n");
    return -1;
  }

  // initialize packet
  initPkt(&out_pkt, TO_NODE_ROUTING_PKT_TYPE);

  // build data payload
  // NOTICE bitwise operation used here
  out_pkt.data[0] = 1;              // # of nodes contained in the packet, max 8
  out_pkt.data[1] = localAddr >> 8; // high bits of address
  out_pkt.data[2] = localAddr;      // low bits of address

  // send routing packet
  if (sendtoBase(&out_pkt, 0) < 0) {
    printf("Failed sending routing packet!\n\n");
    return -1;
  }
  else
    printf("Base to nodes routing packet sent.\n\n");

  return 0;
}


static void updateToBaseTable(nodeProperty src_node, __u16 *canHear)
{
  pthread_mutex_lock(&toBaseRtTableLock);

  int neighbor_can_hear = 0; // flag set if neighbor can hear current node
  int i, j;
  __u16 tmp_nextHop, localAddr;
  __u8 tmp_numHops;
  int tmp_lastHeard;

  // get localAddr
  localAddr = get_addr();

  // if sender is base node
  if (src_node.numHops == 0)
    numHops = 1; // then current node is 1 hop away

  // check if neighbor can hear current node
  for (i = 0; i < ROUTING_TABLE_SIZE; i++) {
    if (localAddr == canHear[i]) {
      neighbor_can_hear = 1;
      break;
    }
  }

  // add new neighbor to routing table
  if (neighbor_can_hear) {
    for (i = 0; i < ROUTING_TABLE_SIZE; i++) {
      // replace larger entry
      if ((toBaseTable[i].numHops >= src_node.numHops) ||
          (toBaseTable[i].nextHop == src_node.addr)) {
        toBaseTable[i].nextHop = src_node.addr;
        toBaseTable[i].numHops = src_node.numHops;
        toBaseTable[i].lastHeard = 0;
        // update current node numHops
        if (numHops >= src_node.numHops + 1)
          numHops = src_node.numHops + 1;
        break;
      }
    }
  }
  // neighbor CANT hear current node, add neighbor to routing table
  else {
    for (i = 0; i < ROUTING_TABLE_SIZE; i++) {
      // replace default entry
      if ((toBaseTable[i].numHops == 255) ||
          (toBaseTable[i].nextHop == src_node.addr)) {
        toBaseTable[i].nextHop = src_node.addr;
        toBaseTable[i].numHops = src_node.numHops;
        toBaseTable[i].lastHeard = 0;
        // update current node numHops
        if (numHops >= src_node.numHops + 1)
          numHops = src_node.numHops + 1;
        break;
      }
    }
  }

  // update lastHeard
  for (i = 0; i < ROUTING_TABLE_SIZE; i++) {
    if (toBaseTable[i].nextHop != BROADCAST_ADDR) // skip empty entries
      toBaseTable[i].lastHeard++;
    if (toBaseTable[i].lastHeard > MAX_STAY_TO_BASE) {
      toBaseTable[i].nextHop = 0x0; // reset to initial
      toBaseTable[i].numHops = 255; // reset to max
      toBaseTable[i].lastHeard = 0;
    }
  }

  // sort routing table (bubble sort)
  for (i = 0; i < ROUTING_TABLE_SIZE; i++) {
    for (j = i + 1; j < ROUTING_TABLE_SIZE; j++) {
      if (toBaseTable[i].numHops > toBaseTable[j].numHops) {
        // swap values
        tmp_nextHop = toBaseTable[i].nextHop;
        tmp_numHops = toBaseTable[i].numHops;
        tmp_lastHeard = toBaseTable[i].lastHeard;
        toBaseTable[i].nextHop = toBaseTable[j].nextHop;
        toBaseTable[i].numHops = toBaseTable[j].numHops;
        toBaseTable[i].lastHeard = toBaseTable[j].lastHeard;
        toBaseTable[j].nextHop = tmp_nextHop;
        toBaseTable[j].numHops = tmp_numHops;
        toBaseTable[j].lastHeard = tmp_lastHeard;
      }
    }
  }

  pthread_mutex_unlock(&toBaseRtTableLock);
}


static void base_recv_rtPkt(packet *in_pkt)
{
  int i;

  // get # of hops
  __u8 length = in_pkt->data[0];
  // obtain routing list
  __u16 *addrList = (__u16 *)malloc((length - 1) * 4);
  for (i = 0; i < length - 1; i++) {
    addrList[i] = (__u16)in_pkt->data[3 + 2 * i] << 8;
    addrList[i] |= (__u16)in_pkt->data[4 + 2 * i];
  }
  // get destination node (1st element in routing list)
  __u16 destAddr = ((__u16)in_pkt->data[1] << 8) | (__u16)in_pkt->data[2];

  printf("Received base routing packet from: %x\n", destAddr);

  // add/update base routing table
  if ((length < MAX_NUM_NODES_IN_RT) && (length != 0))
    append_toNodeRtEntry(&to_node_table, length, destAddr, addrList);
  else
    printf("Maximum number of hops exceeded.\n");
}


static void append_toNodeRtEntry(toNodeRtTable *table, __u8 length, __u16 destAddr, __u16 *addrList)
{
  assert(table);

  pthread_mutex_lock(&table->lock);

  // empty table
  if (table->head == NULL) {
    table->head = malloc(sizeof(toNodeRtEntry));
    table->head->length = length;
    table->head->destAddr = destAddr;
    table->head->addrList = addrList;
    table->head->age = 0;
    table->head->next = NULL;
    table->size = 1;
    pthread_mutex_unlock(&table->lock);
    return;
  }

  // non-empty table
  toNodeRtEntry *cur = table->head;
  toNodeRtEntry *pre = table->head;

  while (cur) {
    // replace existing entry
    if (cur->destAddr == destAddr) {
      free(cur->addrList);  // release old list
      cur->addrList = addrList;
      cur->age = 0;
      cur->length = length;
      pthread_mutex_unlock(&table->lock);
      return;
    }
    pre = cur;
    cur = cur->next;
  }

  // add new entry
  cur = malloc(sizeof(toNodeRtEntry));
  cur->length = length;
  cur->destAddr = destAddr;
  cur->addrList = addrList;
  cur->age = 0;
  cur->next = NULL;
  pre->next = cur;
  table->size++;

  pthread_mutex_unlock(&table->lock);
}


static void check_toNodeRtTable(toNodeRtTable *table)
{
  assert(table);

  pthread_mutex_lock(&table->lock);

  toNodeRtEntry *cur = table->head;
  toNodeRtEntry *pre = table->head;
  toNodeRtEntry *tmp = NULL;

  while (cur) {
    cur->age++;
    if (cur->age > MAX_STAY_TO_NODE) {
      tmp = cur;
      if (cur == table->head) { // special case for changing head
        table->head = table->head->next;
        cur = table->head;
        pre = table->head;
      }
      else {
        pre->next = cur->next;
        cur = cur->next;
      }
      table->size--;  // reduce size of routing table
      free(tmp);
      continue;
    }
    pre = cur;
    cur = cur->next;
  }

  pthread_mutex_unlock(&table->lock);
}


static const toNodeRtEntry *findRtToNode(toNodeRtTable *table, __u16 addr)
{
  assert(table);

  pthread_mutex_lock(&table->lock);

  toNodeRtEntry *cur = table->head;
  while (cur) {
    if (cur->destAddr == addr) {
      pthread_mutex_unlock(&table->lock);
      return cur;
    }
    cur = cur->next;
  }

  pthread_mutex_unlock(&table->lock);

  return NULL;
}


static __u16 findRtToBase()
{
  __u16 destNode = 0x0;
  int i;

  pthread_mutex_lock(&toBaseRtTableLock);
  // read local to base routing table
  for (i = 0; i < ROUTING_TABLE_SIZE; i++) {
    // first non-zero entry is always the shortest
    destNode = toBaseTable[i].nextHop;
    if (destNode != 0x0)
      break;
  }
  pthread_mutex_unlock(&toBaseRtTableLock);

  return destNode;
}
